#include "AutoBreach.h"

AutoBreach::AutoBreach() {
	// Use requires() here to declare subsystem dependencies
	// eg. requires(chassis);
	latch = false;
	oldlatch = false;
	Requires(breach);
}

// Called just before this Command runs the first time
void AutoBreach::Initialize() {
}

// Called repeatedly when this Command is scheduled to run
void AutoBreach::Execute() {
//	if(oi->GetShooterOverride() && !shooter->flipper.Get()){
//		if(oi->shooterJoystick.GetRawButton(3)){
//			breach->breachNUp->StartSingle(0.0);
//		}else{
//			breach->breachNDown->StartSingle(0.0);
//		}
//	}else 
	if (!shooter->flipper.Get()) {
		latch = (discHandling->topProxSensor.Get() == 0
				&& discHandling->breachProxSensor.Get() == 1);
		if (latch && !oldlatch) {
			Breach::Pulse(breach);
		}
//		if (discHandling->topProxSensor.Get()==0)
		oldlatch = latch;
		
		
	}
}

// Make this return true when this Command no longer needs to run execute()
bool AutoBreach::IsFinished() {
	//	if(oi->GetIfFeeder()){
	//		if (discHandling->bottomProxSensor.Get() + discHandling->middleProxSensor.Get() + discHandling->topProxSensor.Get() + discHandling->breachProxSensor.Get() == 0){
	//		return true;
	//		} else {
	//		return false;
	//		}		
	//		}
	//	return (CommandBase::discHandling->breachProxSensor.Get() == 1);
	return false;
}

// Called once after isFinished returns true
void AutoBreach::End() {
	CommandBase::breach->ControlBreachState(CommandBase::breach->kBreachDown);
}

// Called when another command which requires one or more of the same
// subsystems is scheduled to run
void AutoBreach::Interrupted() {
	CommandBase::breach->ControlBreachState(CommandBase::breach->kBreachDown);
}
